#include "SmallRigid.hpp"
#include <zMat.hpp>
#include <Utility/Log.hpp>
#include "../VisionTools.hpp"

namespace zzz{
bool SmallRigid::Create(const vector<Vector2d> &pa, const vector<Vector2d> &pb)
{
  ZCHECK_EQ(pa.size(), pb.size());
  zuint n=pa.size();
  zMatrix<double> A(Zerosd(n*2,3));
  zVector<double> B(n*2);
  zuint cur=0;
  for (zuint i=0; i<n; i++)
  {
    const Vector2d &a=pa[i],&b=pb[i];
    A(cur,0)=-a[1]; A(cur,1)=1; B(cur)=b[0];
    cur++;
    A(cur,0)=a[0];  A(cur,1)=1; B(cur)=b[1]-a[1];
    cur++;
  }
  zMatrix<double> ATA(Trans(A)*A);
  if (!Invert(ATA))
    return false;
  zVector<double> X(ATA*Trans(A)*B);

  double theta = asin(X(0));
  Hab_(0,0)=cos(theta); Hab_(0,1)=-sin(theta); Hab_(0,2)=X(1);
  Hab_(1,0)=sin(theta); Hab_(1,1)=cos(theta); Hab_(1,2)=X(2);
  Hba_=Hab_.Inverted();
  return true;
}
}
